{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Populating the interactive namespace from numpy and matplotlib\n"
     ]
    }
   ],
   "source": [
    "%pylab inline\n",
    "from sympy import init_printing\n",
    "init_printing(use_latex='mathjax')\n",
    "from sympy import Matrix"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "\n",
      "Robotics Toolbox for Python\n",
      "Based on Matlab Toolbox Version 7  April-2002\n",
      "\n",
      "What's new.\n",
      "  Readme      - New features and enhancements in this version.\n",
      "\n",
      "Homogeneous transformations\n",
      "  eul2tr      - Euler angle to transform \n",
      "  oa2tr       - orientation and approach vector to transform \n",
      "  rotx        - transform for rotation about X-axis \n",
      "  roty        - transform for rotation about Y-axis \n",
      "  rotz        - transform for rotation about Z-axis \n",
      "  rpy2tr      - roll/pitch/yaw angles to transform \n",
      "  tr2eul      - transform to Euler angles \n",
      "  tr2rot      - transform to rotation submatrix\n",
      "  tr2rpy      - transform to roll/pitch/yaw angles\n",
      "  transl      - set or extract the translational component of a transform \n",
      "  trnorm      - normalize a transform \n",
      "  \n",
      "Quaternions\n",
      "  /           - divide quaternion by quaternion or scalar\n",
      "  *           - multiply quaternion by a quaternion or vector\n",
      "  inv         - invert a quaternion \n",
      "  norm        - norm of a quaternion \n",
      "  plot        - display a quaternion as a 3D rotation\n",
      "  qinterp     - interpolate quaternions\n",
      "  unit        - unitize a quaternion \n",
      "\n",
      "Kinematics\n",
      "  diff2tr     - differential motion vector to transform \n",
      "  fkine       - compute forward kinematics \n",
      "  ikine       - compute inverse kinematics \n",
      "  ikine560    - compute inverse kinematics for Puma 560 like arm\n",
      "  jacob0      - compute Jacobian in base coordinate frame\n",
      "  jacobn      - compute Jacobian in end-effector coordinate frame\n",
      "  tr2diff     - transform to differential motion vector \n",
      "  tr2jac      - transform to Jacobian \n",
      "  \n",
      "Dynamics\n",
      "  accel       - compute forward dynamics\n",
      "  cinertia    - compute Cartesian manipulator inertia matrix \n",
      "  coriolis    - compute centripetal/coriolis torque \n",
      "  friction    - joint friction\n",
      "  ftrans      - transform force/moment \n",
      "  gravload    - compute gravity loading \n",
      "  inertia     - compute manipulator inertia matrix \n",
      "  itorque     - compute inertia torque \n",
      "  nofriction  - remove friction from a robot object \n",
      "  rne         - inverse dynamics \n",
      "  \n",
      "Trajectory generation\n",
      "  ctraj       - Cartesian trajectory \n",
      "  jtraj       - joint space trajectory \n",
      "  trinterp    - interpolate transform s\n",
      "  \n",
      "Graphics\n",
      "  drivebot    - drive a graphical  robot \n",
      "  plot        - plot/animate robot \n",
      "  \n",
      "Other\n",
      "  manipblty   - compute manipulability \n",
      "  unit        - unitize a vector\n",
      "\n",
      "Creation of robot models.\n",
      "  link        - construct a robot link object \n",
      "  puma560     - Puma 560 data \n",
      "  puma560akb  - Puma 560 data (modified Denavit-Hartenberg)\n",
      "  robot       - construct a robot object \n",
      "  stanford    - Stanford arm data \n",
      "  twolink     - simple 2-link example \n",
      "\n"
     ]
    }
   ],
   "source": [
    "from robot import *"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "import almath\n",
    "import motion\n",
    "import time\n",
    "from naoqi import ALProxy\n",
    "frame = motion.FRAME_TORSO\n",
    "useSensorValues = False\n",
    "motionProxy = ALProxy(\"ALMotion\",\"127.0.0.1\",9559)\n",
    "motionProxy.rest()\n",
    "motionProxy.wakeUp()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "data": {
      "text/plain": [
       "Position6D(x=0.118749, y=0.133122, z=-0.0442597, wx=-1.21697, wy=0.415305, wz=0.0127927)"
      ]
     },
     "execution_count": 4,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "Pos1 = almath.Position6D(motionProxy.getPosition(\"LArm\", frame, useSensorValues))\n",
    "Pos1"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "data": {
      "text/plain": [
       "Position6D(x=0.118749, y=0.133122, z=-0.0442597, wx=-1.21697, wy=0.415305, wz=0.0127927)"
      ]
     },
     "execution_count": 5,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "Pos2=almath.Position6D(Pos1.toVector())\n",
    "Pos2"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 26,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "from time import sleep"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 30,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Position6D(x=0.118749, y=0.133122, z=-0.0442597, wx=-1.21697, wy=0.415305, wz=0.0127927)\n",
      "Position6D(x=0.118887, y=0.133115, z=-0.0442389, wx=-1.21696, wy=0.415301, wz=0.0127986)\n",
      "0.000139846728416\n",
      "Position6D(x=0.138749, y=0.133122, z=-0.0442597, wx=-1.21697, wy=0.415305, wz=0.0127927)\n",
      "Position6D(x=0.137052, y=0.131173, z=-0.0402015, wx=-1.2168, wy=0.415637, wz=0.0130818)\n",
      "0.00481138192117\n",
      "Position6D(x=0.158749, y=0.133122, z=-0.0442597, wx=-1.21697, wy=0.415305, wz=0.0127927)\n",
      "Position6D(x=0.15262, y=0.128249, z=-0.0341056, wx=-1.21655, wy=0.416081, wz=0.0135516)\n",
      "0.0128223672509\n",
      "Position6D(x=0.178749, y=0.133122, z=-0.0442597, wx=-1.21697, wy=0.415305, wz=0.0127927)\n",
      "Position6D(x=0.164908, y=0.12493, z=-0.027172, wx=-1.21632, wy=0.416361, wz=0.0140357)\n",
      "0.0234665460885\n",
      "Position6D(x=0.198749, y=0.133122, z=-0.0442597, wx=-1.21697, wy=0.415305, wz=0.0127927)\n",
      "Position6D(x=0.174262, y=0.12164, z=-0.0202801, wx=-1.21606, wy=0.41653, wz=0.0145817)\n",
      "0.0361455641687\n",
      "Position6D(x=0.218749, y=0.133122, z=-0.0442597, wx=-1.21697, wy=0.415305, wz=0.0127927)\n",
      "Position6D(x=0.181292, y=0.118594, z=-0.0138825, wx=-1.21583, wy=0.416534, wz=0.0151168)\n",
      "0.0503671430051\n",
      "Position6D(x=0.238749, y=0.133122, z=-0.0442597, wx=-1.21697, wy=0.415305, wz=0.0127927)\n",
      "Position6D(x=0.186582, y=0.115876, z=-0.0081519, wx=-1.21562, wy=0.416384, wz=0.0156231)\n",
      "0.0657462924719\n",
      "Position6D(x=0.258749, y=0.133122, z=-0.0442597, wx=-1.21697, wy=0.415305, wz=0.0127927)\n",
      "Position6D(x=0.190597, y=0.113494, z=-0.00310684, wx=-1.21544, wy=0.416102, wz=0.0160947)\n",
      "0.0819967910647\n",
      "Position6D(x=0.278749, y=0.133122, z=-0.0442597, wx=-1.21697, wy=0.415305, wz=0.0127927)\n",
      "Position6D(x=0.193563, y=0.111495, z=0.00111474, wx=-1.21529, wy=0.415764, wz=0.0165129)\n",
      "0.0989105179906\n",
      "Position6D(x=0.298749, y=0.133122, z=-0.0442597, wx=-1.21697, wy=0.415305, wz=0.0127927)\n",
      "Position6D(x=0.195787, y=0.109795, z=0.00461647, wx=-1.21516, wy=0.415384, wz=0.0168922)\n",
      "0.116337262094\n"
     ]
    }
   ],
   "source": [
    "motionProxy.setPositions(\"LArm\",frame,Pos1.toVector(),0.5,almath.AXIS_MASK_ALL)\n",
    "sleep(1)\n",
    "for dx in range(10):\n",
    "    Pos2.x = Pos1.x + dx*0.02\n",
    "    motionProxy.setPositions(\"LArm\",frame,Pos2.toVector(),0.5,almath.AXIS_MASK_ALL)\n",
    "    sleep(1)\n",
    "    curPos = almath.Position6D(motionProxy.getPosition(\"LArm\", frame, useSensorValues))\n",
    "    print Pos2\n",
    "    print curPos\n",
    "    print almath.Position6D.distance(Pos2,curPos)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 32,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Position6D(x=0.118749, y=0.133122, z=-0.0442597, wx=-1.21697, wy=0.415305, wz=0.0127927)\n",
      "Position6D(x=0.118802, y=0.133119, z=-0.0442517, wx=-1.21697, wy=0.415304, wz=0.0127946)\n",
      "5.36368061148e-05\n",
      "Position6D(x=0.138749, y=0.133122, z=-0.0442597, wx=-1.21697, wy=0.415305, wz=0.0127927)\n",
      "Position6D(x=0.137051, y=0.131173, z=-0.0402017, wx=-1.2168, wy=0.415638, wz=0.0130808)\n",
      "0.00481140706688\n",
      "Position6D(x=0.158749, y=0.133122, z=-0.0442597, wx=-1.21697, wy=0.415305, wz=0.0127927)\n",
      "Position6D(x=0.15262, y=0.128249, z=-0.0341057, wx=-1.21655, wy=0.416081, wz=0.0135514)\n",
      "0.012822364457\n",
      "Position6D(x=0.178749, y=0.133122, z=-0.0442597, wx=-1.21697, wy=0.415305, wz=0.0127927)\n",
      "Position6D(x=0.164908, y=0.12493, z=-0.027172, wx=-1.21632, wy=0.416361, wz=0.0140358)\n",
      "0.0234665367752\n",
      "Position6D(x=0.198749, y=0.133122, z=-0.0442597, wx=-1.21697, wy=0.415305, wz=0.0127927)\n",
      "Position6D(x=0.174262, y=0.12164, z=-0.0202801, wx=-1.21606, wy=0.41653, wz=0.0145817)\n",
      "0.0361455604434\n",
      "Position6D(x=0.218749, y=0.133122, z=-0.0442597, wx=-1.21697, wy=0.415305, wz=0.0127927)\n",
      "Position6D(x=0.181292, y=0.118594, z=-0.0138825, wx=-1.21583, wy=0.416534, wz=0.0151168)\n",
      "0.050367154181\n",
      "Position6D(x=0.238749, y=0.133122, z=-0.0442597, wx=-1.21697, wy=0.415305, wz=0.0127927)\n",
      "Position6D(x=0.186582, y=0.115877, z=-0.00815184, wx=-1.21562, wy=0.416384, wz=0.0156231)\n",
      "0.065746307373\n",
      "Position6D(x=0.258749, y=0.133122, z=-0.0442597, wx=-1.21697, wy=0.415305, wz=0.0127927)\n",
      "Position6D(x=0.190597, y=0.113494, z=-0.00310692, wx=-1.21544, wy=0.416103, wz=0.0160947)\n",
      "0.0819967910647\n",
      "Position6D(x=0.278749, y=0.133122, z=-0.0442597, wx=-1.21697, wy=0.415305, wz=0.0127927)\n",
      "Position6D(x=0.193586, y=0.11148, z=0.00114986, wx=-1.21529, wy=0.415755, wz=0.0165168)\n",
      "0.0989102646708\n",
      "Position6D(x=0.298749, y=0.133122, z=-0.0442597, wx=-1.21697, wy=0.415305, wz=0.0127927)\n",
      "Position6D(x=0.195789, y=0.109793, z=0.0046211, wx=-1.21516, wy=0.415382, wz=0.0168927)\n",
      "0.116337172687\n"
     ]
    }
   ],
   "source": [
    "motionProxy.setPositions(\"LArm\",frame,Pos1.toVector(),0.5,almath.AXIS_MASK_ALL)\n",
    "sleep(1)\n",
    "for dx in range(10):\n",
    "    Pos2.x = Pos1.x + dx*0.02\n",
    "    motionProxy.setPositions(\"LArm\",frame,Pos2.toVector(),0.5,almath.AXIS_MASK_ALL)\n",
    "    sleep(1)\n",
    "    curPos = almath.Position6D(motionProxy.getPosition(\"LArm\", frame, useSensorValues))\n",
    "    print Pos2\n",
    "    print curPos\n",
    "    print almath.Position6D.distance(Pos2,curPos)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 2",
   "language": "python",
   "name": "python2"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 2
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython2",
   "version": "2.7.12"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 1
}
